#include <stdio.h>
#include "Nano100Series.h"

#define CS0 PC8
#define CS1	PB9
#define CS2 PB11


void sensor_write_spi(uint8_t ch, uint8_t add, uint8_t data)
{
	if (ch==0) {
		SPI_SET_SS0_LOW(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 1;
	} else if (ch==1) {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_LOW(SPI1);
		CS2 = 1;
	} else {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 0;
	}
		
	SPI_WRITE_TX0(SPI1, add);
    SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));
	
	SPI_WRITE_TX0(SPI1, data);
    SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));
	
	SPI_SET_SS0_HIGH(SPI1);
	SPI_SET_SS1_HIGH(SPI1);
	CS2 = 1;
}

uint8_t sensor_spi_read(uint8_t ch, uint8_t add)
{
	if (ch==0) {
		SPI_SET_SS0_LOW(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 1;
	} else if (ch==1)
		{
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_LOW(SPI1);
		CS2 = 1;
	} else {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 0;
	}

	SPI_WRITE_TX0(SPI1, add|0X80);	
	SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));
	
	SPI_WRITE_TX0(SPI1, 0);	
	SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));
	
	SPI_SET_SS0_HIGH(SPI1);
	SPI_SET_SS1_HIGH(SPI1);
	CS2 = 1;
	
	return SPI_READ_RX0(SPI1);
}

uint8_t sensor_spi_read_multi(uint8_t ch,uint8_t add, uint8_t *buf, uint8_t count)
{
	int i;
	
	if (ch==0) {
		SPI_SET_SS0_LOW(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 1;
	} else if (ch==1)
		{
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_LOW(SPI1);
		CS2 = 1;
	} else {
		SPI_SET_SS0_HIGH(SPI1);
		SPI_SET_SS1_HIGH(SPI1);
		CS2 = 0;
	}
	
	SPI_WRITE_TX0(SPI1, add|0X80);	
	SPI_TRIGGER(SPI1);
	while(SPI_IS_BUSY(SPI1));
	
	for (i = 0; i < count; i++) {
		SPI_WRITE_TX0(SPI1, 0);
		SPI_TRIGGER(SPI1);
		while(SPI_IS_BUSY(SPI1));
		buf[i] = SPI_READ_RX0(SPI1);
	}
	
	SPI_SET_SS0_HIGH(SPI1);
	SPI_SET_SS1_HIGH(SPI1);
	CS2 = 1;
	
	return i;
}

void sensor_init()
{
	/* Configure SPI0 as a master, MSB first, 32-bit transaction, SPI Mode-0 timing, clock is 2MHz */
    //SPI_Open(SPI1, SPI_MASTER, SPI_MODE_0, 16, 2000000);
	SPI_Open(SPI1, SPI_MASTER, SPI_MODE_0, 8, 2000000);

    /* Enable the automatic hardware slave select function. Select the SPI0_SS0 pin and configure as low-active. */
   // SPI_EnableAutoSS(SPI1, SPI_SS1, SPI_SS1_ACTIVE_LOW);
	//SPI_EnableAutoSS(SPI1, SPI_SS0, SPI_SS0_ACTIVE_LOW);
	SPI_DisableAutoSS(SPI1);
	
	GPIO_SetMode(PB, BIT11, GPIO_PMD_OUTPUT);
	CS2 = 1;

    /* Enable SPI0 two bit mode */
    //SPI_ENABLE_2BIT_MODE(SPI1);

	CLK_SysTickDelay(1000);
	/* accelerometer */
	sensor_write_spi(1,0x14,0xB6);
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(1,0x10,0x0C);	// BW : 1000 Hz(0x0F) 125 Hz(0x0C)
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(1,0x0F,0x03);	// RANGE : 2G
	CLK_SysTickDelay(1000);
	
	/* gyroscope */
	sensor_write_spi(0,0x14,0xB6);
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(0,0x10,0x05);	// ODR : 1000 Hz(0x02) 100 Hz(0x05)
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(0,0x0F,0x02);	// RANGE 500 d/s
	CLK_SysTickDelay(1000);
	
	/* magnetic */
	sensor_write_spi(2,0x4B, 0x01);	// suspend -> sleep
	CLK_SysTickDelay(10 * 1000);
	
	sensor_write_spi(2,0x4C, (0x00 << 3) | (0x00 << 1));	// sleep -> normal
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(2,0x51, 7);	// REP XY : 2 * 7 + 1 = 15
	CLK_SysTickDelay(1000);
	
	sensor_write_spi(2,0x52, 26);	// REP Z : 26 + 1 = 27
	CLK_SysTickDelay(1000);
}

